//=================================================================================================
// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Simulation, Systems Optimization and Robotics
//       group, TU Darmstadt nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================

#ifndef HECTOR_MAPPING_ROS_H__
#define HECTOR_MAPPING_ROS_H__

#include <boost/thread.hpp>

#include <ros/ros.h>

#include <nav_msgs/OccupancyGrid.h>
#include <std_msgs/String.h>

#include <laser_geometry/laser_geometry.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

#include "google_mapping/ScanMatcher.h"
#include "google_mapping/MapVisualization.h"

class HectorMappingRos {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  HectorMappingRos();

 private:
  void scanCallback(const sensor_msgs::LaserScan& scan);
  void cloudCallback(const sensor_msgs::PointCloud2& cloud);
  void processPointcloud(pcl::PointCloud<pcl::PointXYZ>& point_cloud, const ros::Time& acquisition_stamp);
  void syscommandCallback(const std_msgs::String& string);
  void getOccupancyGrid(
      const ros::Time& time, nav_msgs::OccupancyGrid* message);
  void publishMap(const ros::Time& timestamp);
  void publishTransformLoop(double p_transform_pub_period_);
  void publishMapLoop(double p_map_pub_period_);
  void publishTransform();
  bool poseDifferenceLargerThan(
      const tf::Pose& pose1,
      const tf::Pose& pose2,
      double distance_threshold,
      double angle_threshold);

  std::string p_base_frame_;
  std::string p_map_frame_;
  double p_map_pub_period_;
  double p_map_resolution_;
  double p_map_hit_probability_;
  double p_map_miss_probability_;
  double p_map_minimum_occupancy_;
  double p_map_maximum_occupancy_;
  double p_laser_z_max_value_;
  double p_laser_z_min_value_;

  boost::mutex mutex_;
  boost::scoped_ptr<boost::thread> map_publisher_thread_;
  hectorslam::ScanMatcher scan_matcher_;
  laser_geometry::LaserProjection laser_projection_;
  ros::NodeHandle node_;
  std::vector<Eigen::Vector2f> scan_;
  tf::Pose pose_;
  tf::Pose last_inserted_point_cloud_pose_;
  tf::TransformBroadcaster tf_broadcaster_;
  tf::TransformListener tf_listener_;

  ros::Subscriber scan_subscriber_;
  ros::Subscriber pointcloud_subscriber_;
  ros::Subscriber syscommand_subscriber_;
  boost::scoped_ptr<google_mapping::Map> map_;
  boost::scoped_ptr<google_mapping::MapVisualization> map_vis_;
  ros::Publisher map_publisher_;

  bool first_point_cloud_inserted_;
  ros::Time last_inserted_scan_time_;
  hectorslam::HumanMotionModel motion_model_;
};

#endif  // HECTOR_MAPPING_ROS_H_
